//
// Created by rebeater on 2020/11/4.
//

#include "WGS84.h"
#include <cmath>

double WGS84::RM(double lat) const {
    return r * (1 - e2) * pow(1 - e2 * sin(lat) * sin(lat), 1.5);
}

double WGS84::RN(double lat) const {
    return r / (sqrt(1 - e2 * sin(lat) * sin(lat)));
}

double WGS84::dN(double lat1, double lat2) const {
    return (lat1 - lat2) * RM(lat1);/*todo 或许应该用(lat1+lat2)/2*/
}

double WGS84::dE(double lat, double lon1, double lon2) const {
    return RN(lat) * cos(lat) * (lon1 - lon2);
}

deltaPos WGS84::distance(double lat1, double lon1, double lat2, double lon2) const {
    auto dn = dN(lat1, lat2);
    auto de = dE(lat1, lon1, lon2);
    return {dn, de, 0, sqrt(dn * dn + de * de)};
};

deltaPos WGS84::distance(double lat1, double lon1, double lat2, double lon2, double h1, double h2) const {
    auto dn = dN(lat1, lat2);
    auto de = dE(lat1, lon1, lon2);
    auto dd = h1 - h2;
    return {dn, de, dd, sqrt(dn * dn + de * de + dd * dd)};
};


deltaPos WGS84::distance(GnssData pos1, GnssData pos2) const {
    return distance(pos1.lat, pos1.lon, pos2.lat, pos2.lon);
}

deltaPos WGS84::distance(NavEpoch nav1, NavEpoch nav2) const {
    return distance(nav1.lat, nav1.lon, nav2.lat, nav2.lon, nav1.h, nav2.h);
}

void WGS84::update_g(double lat, double h) {
    double s = sin(lat);
    double s2 = s * s;
    g = g0 * (
            1 + 0.0052790414 * s2 + 0.0000232718 * s2 * s2) + h * (
            0.0000000043977311 * s2 - 0.0000030876910891
    ) + 0.0000000000007211 * h * h;

}

WGS84 wgs84;